자세 계산 알고리즘(ARS) 실행속도 개선

□  현용 자세 계산 알고리즘은 대부분 작업을 double로 계산하는데, 이를 float 연산으로 변경하면 소요시간을 57% 단축함

□  소요시간 비교

o 사용 MCU : nRF52832(Clock 64MHz)

o 비교방식 : 알고리즘으로 720개 샘플을 10회 처리하고 소요시간을 측정

o 소요시간

구 분	FPU	전체 소요시간(ms)	샘플당 소요시간(ms)	비 고
double	○	9,845	1.3674	bars-0.0.1
float	○	4,186	0.5814	bars-1.0.0
double	×	10,073	1.3990
float	×	4,773	0.6629

* FPU(Floating Point Unit) : 부동소수점 처리장치

* double : 64 비트(S/W로 계산), float : 32비트(FPU로 고속 계산)

□  계산결과 비교(FPU 사용)

o Roll

o Pitch

o Yaw

□ 결과 분석 및 조치

o FPU를 사용해서 float 연산으로 자세를 계산하는 방식이 소요시간을 54% 단축하고 자세 계산결과도 기존 계산 방식과 동일함

o 향후 자세 계산에 float 연산 방식을 사용하겠음. 끝.

알고리즘 실행속도 비교 참고자료

□ 실행속도 테스트 코드

int

bars_test() {

bars obj;

bars_init(&obj, 0.020f);

InitLPF();

pimu_int16_data pData;

float acc1[3] = { 0.0f, 0.0f, 0.0f };

float acc2[3] = { 0.0f, 0.0f, 0.0f };

float acc3[3] = { 0.0f, 0.0f, 0.0f };

float gyro1[3] = { 0.0f, 0.0f, 0.0f };

float gyro2[3] = { 0.0f, 0.0f, 0.0f };

float gyro3[3] = { 0.0f, 0.0f, 0.0f };

uint32_t start_tick = sys_tick_get_tick();

for(int i = 0; i < 10; i++) {

for(int j = 0; j < SAMPLE_DATA_NUM; j++) {

pData = (pimu_int16_data) &sample_data[j];

imu_float_data  data;

data.ax = ((float)(pData->ax)) / 8192.0f; // +- 4G

data.ay = ((float)(pData->ay)) / 8192.0f;

data.az = ((float)(pData->az)) / 8192.0f;

data.gx = ((float)(pData->gx)) / 65.536f; // +- 500 DPS

data.gy = ((float)(pData->gy)) / 65.536f;

data.gz = ((float)(pData->gz)) / 65.536f;

LPF(&data.ax, &acc1[0], &acc2[0], &acc3[0]);

LPF(&data.ay, &acc1[1], &acc2[1], &acc3[1]);

LPF(&data.az, &acc1[2], &acc2[2], &acc3[2]);

LPF(&data.gx, &gyro1[0], &gyro2[0], &gyro3[0]);

LPF(&data.gy, &gyro1[1], &gyro2[1], &gyro3[1]);

LPF(&data.gz, &gyro1[2], &gyro2[2], &gyro3[2]);

bars_work(&obj, acc1, gyro1);

}

}

csprintf("duration : %dms.\r\n", sys_tick_get_tick() - start_tick);

return 0;

}

□ FPU 사용 설정(IAR Embedded Workbench)

자세 기준 벡터(g0) 조정

□ 기존 알고리즘은 정상 상태에서의 중력가속도 작용을 표시하는 자세 기준 벡터(g0)로 (0, 0, -1)을 사용하고 있으나,

o 세이프웨어 센서 모듈은 정상 상태에서 AY가 –1g를 가지므로, 자세 기준 벡터로 (0, -1, 0)을 사용하는 것이 타당함

□ 자세 기준 벡터를 (0, -1, 0)로 변경하면, 자세 기준의 변화에 따라 기존 Roll  → 신규 Yaw로, 기존 Yaw → - 신규 Roll로 변화됨

o 이에 따라 각도 계산 루틴를 다음과 같이 변경함

roll  = (int) -(atan2f((2.0f * (q[1] * q[2] + q[0] * q[3])), (float) (2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f)) * RAD_TO_DEG);

pitch = (int) (atan2f((2.0f * (q[2] * q[3] + q[0] * q[1])), (float) (2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f)) * RAD_TO_DEG);

yaw   = (int) (asinf((2.0f * (q[0] * q[2] - q[1] * q[3]))) * RAD_TO_DEG);

□ 계산 결과 비교

o Roll 비교

o Pitch 비교

o Yaw 비교